import sensor, image, time,pyb
from pyb import Servo
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
thresholds = [
              #(0, 25, 28, 49, 41, 90),
              #(14, 87, -83, 127, -113, -14),
              #(30, 44, -46, -9, 7, 44),
              (0, 80, -127, -10, 0, 127)] #三个阈值更准确些
led = pyb.LED(2) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4.
led.on()            #亮灯

#s1 = Servo(1) # P7 左右控制
#s2 = Servo(2) # P8 上下控制

#s1.angle(0) #舵机1转动到0°
#s2.angle(145) #舵机1转动到0°
clock = time.clock()

while(True):
    clock.tick()
    print(clock.fps())
    img = sensor.snapshot().lens_corr(1.8)
    for r in img.find_rects(threshold = 10000):
        #img.draw_rectangle(r.rect(), color = (0, 0, 0))
        area_rect = (r.rect())   #获取矩形的RIO
        statistics_rect = img.get_statistics(roi=area_rect)#像素颜色统计
        #print(statistics)
        #print(r)

        #l_mode()，a_mode()，b_mode()是L通道，A通道，B通道的众数。
        if 0<statistics_rect.l_mode()<100 and 0<statistics_rect.a_mode()<127 and 0<statistics_rect.b_mode()<127: #红色矩形
            img.draw_rectangle(r.rect(), color = (255, 0, 0))
            img.draw_string(r.x()-10, r.y() - 10, "red_rect", color = (0xFF, 0x00, 0x00))

            deflection_math= 0
            A=deflection_math
            for blob in img.find_blobs(thresholds,pixels_threshold=200,area_threshold=200):
                if blob.density()>0.80:
                    A=0
                elif blob.density()>0.60:
                    A= 0
                elif blob.density()>0.40:
                    A= 3
                    print("检测为红色矩形绿色三角型 ",end='')
                    img.draw_string(blob.x()-10, blob.y() - 10, "green_triangle", color = (0x00, 0xff, 0x00))
                    img.draw_cross(blob.cx(), blob.cy())
                    #print('三角型边长',blob.w())
                else:
                    A= 0
                    #print("no dectedtion")
                if blob.w()<78:
                    A= 0
                if blob.w()>100:
                    A= 0

            for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,r_min = 2, r_max = 100, r_step = 2):
                #img.draw_circle(c.x(), c.y(), c.r(), color = (0, 0, 255))
                area_circle = (c.x()-c.r(), c.y()-c.r(), 2*c.r(), 2*c.r())
                statistics_circle = img.get_statistics(roi=area_circle)#像素颜色统计

                #(100, 0, -127, 0, 127, 0)黄色
                if 0<statistics_circle.l_mode()<100 and -127<statistics_circle.a_mode()<0 and 0<statistics_circle.b_mode()<127:
                    img.draw_circle(c.x(), c.y(), c.r(), color = (255, 255, 0))#识别到的黄色圆形用黄色的圆框出来
                    img.draw_string(c.x()-20, c.y() - 10, "yellow_circle", color = (0xff, 0xff, 0x00))
                    print("检测为红色矩形黄色圆形 ",end='')


        if 0<statistics_rect.l_mode()<100 and 0<statistics_rect.a_mode()<127 and -127<statistics_rect.b_mode()<0: #蓝矩形
            img.draw_rectangle(r.rect(), color = (0, 0, 255))
            img.draw_string(r.x()-10, r.y() - 10, "blue_rect", color = (0xFF, 0x00, 0x00))

            deflection_math= 0
            A=deflection_math
            for blob in img.find_blobs(thresholds,pixels_threshold=200,area_threshold=200):
                if blob.density()>0.80:
                    A=0
                elif blob.density()>0.60:
                    A= 0
                elif blob.density()>0.40:
                    A= 3
                    print("检测为蓝色矩形绿色三角型  ",end='')
                    img.draw_string(blob.x()-10, blob.y() - 10, "green_triangle", color = (0x00, 0xff, 0x00))
                    img.draw_cross(blob.cx(), blob.cy())
                    #print('三角型边长',blob.w())
                else:
                    A= 0
                    #print("no dectedtion")
                if blob.w()<78:
                    A= 0
                if blob.w()>100:
                    A= 0


            for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,r_min = 2, r_max = 100, r_step = 2):
                #img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))
                area_circle = (c.x()-c.r(), c.y()-c.r(), 2*c.r(), 2*c.r())
                statistics_circle = img.get_statistics(roi=area_circle)#像素颜色统计

                #(100, 0, -127, 0, 127, 0)黄色
                if 0<statistics_circle.l_mode()<100 and -127<statistics_circle.a_mode()<0 and 0<statistics_circle.b_mode()<127:
                    img.draw_circle(c.x(), c.y(), c.r(), color = (255, 255, 0))#识别到的黄色圆形用, 你圆框出来
                    img.draw_string(c.x()-20, c.y() - 10, "yellow_circle", color = (0xff, 0xff, 0x00))
                    print("检测为蓝色矩形黄色圆形 ",end='')


